#pragma config(Motor,  port2,           LeftBaseFront, tmotorVex393HighSpeed_MC29, openLoop, reversed, driveLeft)
#pragma config(Motor,  port3,           LeftBaseBack,  tmotorVex393HighSpeed_MC29, openLoop, reversed, driveLeft)
#pragma config(Motor,  port4,           RightBaseFront, tmotorVex393HighSpeed_MC29, openLoop, driveRight)
#pragma config(Motor,  port5,           RightBaseBack, tmotorVex393HighSpeed_MC29, openLoop, driveRight)
#pragma config(Motor,  port6,           ScissorLeft,   tmotorVex393_MC29, openLoop, reversed, driveLeft)
#pragma config(Motor,  port7,           ScissorRight,  tmotorVex393_MC29, openLoop, driveRight)
#pragma config(Motor,  port8,           ArmLeft,       tmotorVex393_MC29, openLoop, driveLeft)
#pragma config(Motor,  port9,           ArmRight,      tmotorVex393_MC29, openLoop, reversed, driveRight)
#pragma config(Motor,  port10,          Claw,          tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;

	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

void AMoveForward(int Time)
{
	motor[LeftBaseBack] = 127;
	motor[LeftBaseFront] = 127;
	motor[RightBaseBack] = 127;
	motor[RightBaseFront] = 127;
	wait1Msec(Time);
	motor[LeftBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[RightBaseFront] = 0;
}

task autonomous()
{
	AMoveForward(5000);
}

//begin user control code

void UDrive()
{
	//forward and backward
	motor(LeftBaseFront) = vexRT(Ch3);
	motor(LeftBaseBack) = vexRT(Ch3);
	motor(RightBaseBack) = vexRT(Ch3);
	motor(RightBaseFront) = vexRT(Ch3);

	//turning
	if(vexRT(Ch1) < 0)
	{
		motor(LeftBaseFront) = vexRT(Ch3);
		motor(LeftBaseBack) = vexRT(Ch3);
		motor(RightBaseBack) = -abs(vexRT(Ch3));
		motor(RightBaseFront) = -abs(vexRT(Ch3));
	}
	if(vexRT(Ch1) > 0)
	{
		motor(LeftBaseFront) = -abs(vexRT(Ch3));
		motor(LeftBaseBack) = -abs(vexRT(Ch3));
		motor(RightBaseBack) = vexRT(Ch3);
		motor(RightBaseFront) = vexRT(Ch3);
	}
	//side to side
	motor(LeftBaseBack) = -vexRT(Ch4);
	motor(LeftBaseFront) = vexRT(Ch4);
	motor(RightBaseBack) = vexRT(Ch4);
	motor(RightBaseFront) = -vexRT(Ch4);
}

void UScissor()
{
	motor[ScissorLeft] = vexRT[Ch3Xmtr2] + 10;
	motor[ScissorRight] = vexRT[Ch3Xmtr2] + 10;
}

void UArms()
{
	motor[ArmLeft] = vexRT[Ch2Xmtr2] + 10;
	motor[ArmRight] = vexRT[Ch2Xmtr2] + 10;
}


void UClaw()
{
	if(vexRT[Btn6UXmtr2] == 1)
	{
		motor[Claw] = 127;
	}
	if(vexRT[Btn6DXmtr2] == 1)
	{
		motor[Claw] = -127;
	}
	if(vexRT[Btn6UXmtr2] == 0 && vexRT[Btn6DXmtr2] == 0)
	{
		motor[Claw] = 0;
	}
}


task usercontrol()
{
	while(1)
	{
		UDrive();
		UScissor();
		UArms();
		UClaw();
	}
}
